#ifndef _H_PLANIFICATION
#define _H_PLANIFICATION

//Bibliotheques generiques
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <math.h>
#include <pthread.h>
#include <sys/time.h>
#include "infos_robot.h"




//Inclusion des bibliotheques pour CFSQP
#include "CFSQP/cfsqpusr.h"

 
//Definitions et structures pour la planification de trajectoire
#define NB_PTS 1000
#define NB_OBS 1000
#define NB_ROBOTS 1000



#define DIST_TRAJ_MAX 5 // l'éloignement entre trajectoire de référence et trajectoire calculée

#define DIST_OBS 0.5// distance minimale robot-obstacle

#define max(x,y) ((x>y) ? x : y)
#define min(x,y) ((x<y) ? x : y)



int world[TAILLE_TABLEAU_MONDE][TAILLE_TABLEAU_MONDE];
//double world[TAILLE_MONDE/PAS_METRIQUE][TAILLE_MONDE/PAS_METRIQUE];

   //Nombre de pas sur l'horizon de plannification

#ifdef __cplusplus
extern "C"
{
#endif
int optimisation(double init[4], double objectif[3], double previous_polynome[8]);
#ifdef __cplusplus
}
#endif

// caractéristiques du robot

//Vitesse maximale d'un robot
double VMAX;
//Vitesse minimale du robot pendant le parcours pour eviter les singularites
double VMIN;
//acceleration maximale d'un robot
double AMAX;
double AMIN;
//Vitesse de rotation maximale d'un robot
double WMAX;


double fin[3];
int fin_experience;
double position_robot[5];


int etat_cooperation;
double fin_cooperation[3];
double distance_initiale;
int encombrement;
int encombrement_vois[NB_ROBOTS];
double nombre_de_point;

double VALEUR_PAS; // pas de temps utilisé
double VALEUR_PAS_APPLICATION;
int Nutil_application;
int Nfinal_pred; // l'horizon sur lequel sont évaluées les contraintes
int Nutil; // l'horizon sur lequel est évalué l'objectif ( et qui correspond à la partie appliquée du polynome )
int mode_resolution; // choix d'un jeu de contraintes et d'une fonction objectif

int nombre_points; // nombres de points de contrôle à côté desquels doit passer le polynome
double position_point[NB_PTS][2]; // tableau qui décrit les points de contrôles ( x , y )
double poids_point[NB_PTS]; // tableau qui décrit le poids des points de contrôles

int nb_obstacle; // nombre d'obstacles à éviter
double coord_obstacle[NB_OBS][2]; // coordonnées des obstacles (x , y)
int nb_obstacle_rect;
double obstacle_rect[NB_OBS][4];
double dist_obs;

double planif[9]; // tableau contenant le polynome à optimiser

int nombre_voisins; // nombre de voisins à éviter
double voisins[NB_ROBOTS][10]; // polynome de chaque voisin
double temps_voisins[NB_ROBOTS][3]; // polynome de chaque voisin
double dist_robot_min; // distance minimale inter robot

#endif
